//////////////////////////////////////////////////////////////////////////
//
//		FileName	-	kdTree3D.cpp
//		Programmer	-	Sujal Bista
//
//		templatized 3D spatial structure
//
///////////////////////////////////////////////////////////////////////////

#include "kdTree3D.h"

 // debug memory allocation enhancement (see next tip)
#ifdef _DEBUG
	#include <crtdbg.h>
	#define DEBUG_NEW new(_NORMAL_BLOCK ,__FILE__, __LINE__)
#else
	#define DEBUG_NEW new
#endif

using namespace kdTreeData3D;

//default const
kdTree3D::kdTree3D(void)
{
	count=0;
	
	this->maxAllowedHeight=14;
	this->optimunDataCount=10;

	setBoundary(0,0,0,1,1,1);

	//dimension of the data
	dataDimension=3;

	data=NULL;
	treeData=NULL;
	indices=NULL;

	//count of stackless data
	treeDataCount=0;

	//height of the tree
	treeHeight=0;
}

//constructor
kdTree3D::kdTree3D(const float &minX,const float &minY,const float &minZ,const float &maxX,const float &maxY,const float &maxZ,unsigned int maxAllowedHeight,unsigned int optimunDataCount)
{
	count=0;
	
	this->maxAllowedHeight=maxAllowedHeight;
	this->optimunDataCount=optimunDataCount;

	setBoundary(minX,minY,minZ,maxX,maxY,maxZ);

	//dimension of the data
	dataDimension=3;

	data=NULL;
	treeData=NULL;
	indices=NULL;

	//count of stackless data
	treeDataCount=0;

	//height of the tree
	treeHeight=0;
}
//sets boundary
void kdTree3D::setBoundary(const float &minX,const float &minY,const float &minZ,const float &maxX,const float &maxY,const float &maxZ)
{
	boundary.minX=minX;
	boundary.minY=minY;
	boundary.minZ=minZ;
	
	boundary.maxX=maxX;
	boundary.maxY=maxY;	
	boundary.maxZ=maxZ;
}
//add a data
bool kdTree3D::add(floatFace *inData, unsigned int inDataCount)
{
	//get full height
	unsigned int maxHeight=MIN((unsigned)ceil(log((float)inDataCount)/log(2.0f)),this->maxAllowedHeight);
	//max count of stackless data
	unsigned int tDataSize=getTotalNodes(maxHeight);
	
	//allocate data for the stackless struct
	levelNodeData *tData=new levelNodeData[tDataSize];
	//levelNodeData *tData=(levelNodeData *)new char[tDataSize*sizeof(levelNodeData)];
	//memset(tData,0,tDataSize*sizeof(levelNodeData));

	//allocate space to hold child indices
	unsigned int childIndexCount=inDataCount*2;
	unsigned int freeChildIndicesIndex=0;	//where the index for child indices 
	unsigned int occupiedChildIndicesIndex=inDataCount;	//where the index for child indices 
	indices=new unsigned int[childIndexCount];
	for(unsigned int i=0;i<inDataCount;i++)
		indices[i]=i;

	//copy the triangles to local data
	//allocate data for elements
	data=(floatFace *)new char[sizeof(floatFace)*inDataCount];
	//copy the input data
	memcpy(data,inData,sizeof(floatFace)*inDataCount);

	unsigned int levelStartIndex=0;
	unsigned int levelNodeCount=1;
	//no of levels
	unsigned int level=0;
	
	
	//make first stackless data
	tData[levelStartIndex].boundary=boundary;
	tData[levelStartIndex].index=0;
	tData[levelStartIndex].childIndex=0;
	tData[levelStartIndex].count=inDataCount;
	tData[levelStartIndex].isLeaf=true;

	
	/////////////////////////////////////////////////////////////////////
	//make different levels
	//make the tree
	do
	{
		//find the median
		findMedianCPU(&tData[levelStartIndex],levelNodeCount,data);

		//calculate nodes for next round
		unsigned int nextLevelStartIndex=levelStartIndex+levelNodeCount;
		unsigned int nextLevelNodeCount=0;	//maximum nodes that will be added

		//split the data and mark node as leaf if node cannot be divided into childrens
		//add more node if needed
		splitCPU(tData, tDataSize,levelStartIndex, levelNodeCount, nextLevelStartIndex, nextLevelNodeCount,indices,childIndexCount,freeChildIndicesIndex,occupiedChildIndicesIndex,data);

		//copy data for next level
		levelStartIndex=nextLevelStartIndex;
		levelNodeCount=nextLevelNodeCount;

		//increase the node
		level++;
	}while(level<maxAllowedHeight && levelNodeCount);

	/////////////////////////////////////////////////////////////////////
	//copy the data to the stackless structure

	//count of stackless data
	treeDataCount=levelStartIndex+levelNodeCount;

	unsigned int indicesCount=0;
	//count the total size of the indices
	for(unsigned int i=0;i<treeDataCount;i++)
		indicesCount+=tData[i].count;

	//allocate data for the stackless struct
	//this->treeData=(stacklessNodeData *)new char[treeTailIndex*sizeof(stacklessNodeData)];
	treeData=new stacklessNodeData[treeDataCount];
	//copy all the data
	for(unsigned int i=0;i<treeDataCount;i++)
	{
		treeData[i].boundary=tData[i].boundary;
		treeData[i].index=tData[i].index;
		treeData[i].count=tData[i].count;
		treeData[i].isLeaf=tData[i].isLeaf;
	
		//copy the indices
		if(treeData[i].count)
			treeData[i].index=tData[i].childIndex;
	}
	delete[] tData;
	
	//copy all the indicies
	unsigned int *tempIndices=indices;
	indices=new unsigned int[indicesCount];
	memcpy(indices,tempIndices,occupiedChildIndicesIndex*sizeof(unsigned int));
	delete[] tempIndices;

	/////////////////////////////////////////////////////////////////////

	//total count
	count=inDataCount;
	treeHeight=level+1;
	return true;
}
//prints to the output stream
std::ostream &kdTree3D::print(std::ostream &out)
{
	if(count==0)
		return out;
	return printHelper(out,treeData,0,data,indices,0);
}
//searches for the node
bool kdTree3D::find(floatFace *keys, unsigned int keysCount,int *&resultIndices)
{
	if(count==0)
		return false;

	//do cpu search
	resultIndices=findHelperCPU(keys,keysCount);
	
	return true;	
}
//searches for the node
bool kdTree3D::findNearestK(float3 *keys, unsigned int keysCount,unsigned int k,int *&resultIndices)
{
	if(count==0)
		return false;

	//do cpu search
	resultIndices=findNearestKHelperCPU(keys,keysCount,k);

	return true;
}	
//searches for the node within the given radius
bool kdTree3D::findRadius(float3 *keys, unsigned int keysCount,float radius,int *&resultIndices,unsigned int *&resultsPosition,unsigned int &resultsCount)
{
	if(count==0)
		return false;

	//do cpu search
	findRadiusHelperCPU(keys,keysCount,radius,resultIndices,resultsPosition,resultsCount);

	return true;
}
//intersect ray
bool kdTree3D::intersectRays(floatRayData *rays,unsigned int raysCount,unsigned int k,int *&resultIndices, float *&t)
{
	if(count==0)
		return false;

	//array to save the right tree indices
	int *farChildrenIndices=new int[treeHeight];

	//temp variables
	int *tempIndices=new int[k];
	float *distance=new float[k];

	//search for each keys
	for(unsigned int pos=0;pos<raysCount;pos++)
	{
		//intersect ray cpu helper
		intersectRayHelperCPU(rays[pos],k,&resultIndices[pos*k],&t[pos*k],farChildrenIndices,tempIndices,distance);
	}


	delete[] tempIndices;
	delete[] distance;
	delete[] farChildrenIndices;

	return true;
}

//intersect individual ray
bool kdTree3D::intersectRay(floatRayData &currRay,int &resultIndex, float &t)
{
	if(count==0)
		return false;

	//array to save the right tree indices
	int *farChildrenIndices=new int[treeHeight];

	int tempIndex=-1;
	t=100000000000.0f;

	//change this
	unsigned int nodeIndex=0;
	int level=0;
	bool firstLeafReached=false;

	while(level>=0)
	{
		bool goLevelUp=false;
		//check if the node is a leaf
		if(treeData[nodeIndex].isLeaf)
		{
			firstLeafReached=true;
			unsigned int startIndex=(unsigned)treeData[nodeIndex].index;
			unsigned int endIndex=startIndex+treeData[nodeIndex].count;

			for(unsigned int i=startIndex;i<endIndex;i++)
			{
				float currDistance=data[indices[i]].getIntersectDistance(currRay);

				//skip if the distance is less than zero
				if(currDistance<0)
					continue;

				//is the index is bigger than current radius
				if(t<currDistance)
					continue;

				//save index and the distance
				tempIndex=indices[i];
				t=currDistance;
			}

			//go level up
			goLevelUp=true;
		}
		else if(firstLeafReached)	//if the first leaf has not been reached we need to find it
		{
			//compute the starting index of the children
			unsigned int leftChildNodeIndex=treeData[nodeIndex].index;
			unsigned int rightChildNodeIndex=leftChildNodeIndex+1;

			float lDistance=treeData[leftChildNodeIndex].boundary.getIntersectDistance(currRay);
			float rDistance=treeData[rightChildNodeIndex].boundary.getIntersectDistance(currRay);

			if(lDistance>=0 && lDistance<t)
			{
				if(rDistance>=0 && rDistance<t)
				{
					if(lDistance<rDistance)
					{
						nodeIndex=leftChildNodeIndex;
						farChildrenIndices[level]=rightChildNodeIndex;
					}
					else
					{
						farChildrenIndices[level]=leftChildNodeIndex;
						nodeIndex=rightChildNodeIndex;
					}
					//increase the level
					level++;
				}
				else
				{
					nodeIndex=leftChildNodeIndex;
				}
			}
			else if(rDistance>=0 && rDistance<t)
			{
				nodeIndex=rightChildNodeIndex;
			}
			else
			{
				//go level up
				goLevelUp=true;
			}


		}
		else
		{
			//compute the starting index of the children
			unsigned int leftChildNodeIndex=treeData[nodeIndex].index;
			unsigned int rightChildNodeIndex=leftChildNodeIndex+1;

			float lDistance=treeData[leftChildNodeIndex].boundary.getIntersectDistance(currRay);
			float rDistance=treeData[rightChildNodeIndex].boundary.getIntersectDistance(currRay);

			if(lDistance>=0)
			{
				if(rDistance>=0)
				{
					if(lDistance<rDistance)
					{
						nodeIndex=leftChildNodeIndex;
						farChildrenIndices[level]=rightChildNodeIndex;
					}
					else
					{
						farChildrenIndices[level]=leftChildNodeIndex;
						nodeIndex=rightChildNodeIndex;
					}
					//increase the level
					level++;
				}
				else
				{
					nodeIndex=leftChildNodeIndex;
				}
			}
			else if(rDistance>=0)
			{
				nodeIndex=rightChildNodeIndex;
			}
			else
			{
				//go level up
				goLevelUp=true;
			}
		}

		if(goLevelUp)
		{
			//decrease level and
			//check if the node intersects the search radius
			do
			{
				//decrease the level
				level--;

				//get out of the whole loop
				if(level<0)
					break;

				//else retrive the other child
				nodeIndex=farChildrenIndices[level];

				float dist=treeData[nodeIndex].boundary.getIntersectDistance(currRay);

				//check if the distance to the box is bigger than expected
				if(dist>-1 && dist<t)
					break;
			}while(true);
		}
	}
	delete[] farChildrenIndices;

	if(tempIndex<0)
		return false;

	resultIndex=tempIndex;
	return true;
}
//returns total number of nodes in the tree
int kdTree3D::getTotalNodes(unsigned int height)
{
	return ((int)(pow(2.0f,(int)(height+1))-1)/1);
}
//clear the entire tree
void kdTree3D::clear(void)
{
	count=0;

	delete[] data;
	delete[] treeData;
	delete[] indices;

	data=NULL;
	treeData=NULL;
	indices=NULL;

	//height of the tree
	treeHeight=0;

	//count of stackless data
	treeDataCount=0;
}
//destructor
kdTree3D::~kdTree3D()
{
	clear();
}
//find the median
void kdTree3D::findMedianCPU(kdTreeData3D::levelNodeData *tData,unsigned int &dataCount,floatFace *data) const
{
	//process all the nodes
	for(unsigned int i=0;i<dataCount;i++)
	{
		//get the current data
		levelNodeData &currTData=tData[i];
		if(!currTData.count)
			continue;

		//compute bounding box
		floatBox bounds=data[indices[currTData.childIndex]].getBoundingBox();
		for(unsigned int j=1;j<currTData.count;j++)
		{
			floatBox currBounds=data[indices[currTData.childIndex+j]].getBoundingBox();

			//compute global bounds
			bounds.minX=MIN(bounds.minX,currBounds.minX);
			bounds.minY=MIN(bounds.minY,currBounds.minY);
			bounds.minZ=MIN(bounds.minZ,currBounds.minZ);
			bounds.maxX=MAX(bounds.maxX,currBounds.maxX);
			bounds.maxY=MAX(bounds.maxY,currBounds.maxY);
			bounds.maxZ=MAX(bounds.maxZ,currBounds.maxZ);
		}		

		//make sure that the bounds are within the nodes bounds
		bounds.minX=MAX(currTData.boundary.minX,bounds.minX);
		bounds.minY=MAX(currTData.boundary.minY,bounds.minY);
		bounds.minZ=MAX(currTData.boundary.minZ,bounds.minZ);
		bounds.maxX=MIN(currTData.boundary.maxX,bounds.maxX);
		bounds.maxY=MIN(currTData.boundary.maxY,bounds.maxY);
		bounds.maxZ=MIN(currTData.boundary.maxZ,bounds.maxZ);
		
		float diffX=bounds.maxX-bounds.minX;
		float diffY=bounds.maxY-bounds.minY;
		float diffZ=bounds.maxZ-bounds.minZ;

		if(diffX>diffY && diffX>diffZ)
		{
			currTData.dividingAxis=0;
			currTData.median=bounds.minX+diffX*.5f;
		}
		else if(diffY>diffZ)
		{
			currTData.dividingAxis=1;
			currTData.median=bounds.minY+diffY*.5f;
		}
		else
		{
			currTData.dividingAxis=2;
			currTData.median=bounds.minZ+diffZ*.5f;
		}
	}
}
//prints to the output stream
std::ostream &kdTree3D::printHelper(std::ostream &out,stacklessNodeData *treeData,unsigned int nodeIndex,floatFace *data,unsigned int *indices,unsigned int level) const
{
	//fix tabs
	for(unsigned int j=0;j<level;out<<"\t",j++);
	out<<treeData[nodeIndex].boundary<<"\n";

	//check if the node is a leaf
	if(treeData[nodeIndex].isLeaf)
	{
		unsigned int startIndex=(unsigned)treeData[nodeIndex].index;
		unsigned int endIndex=startIndex+treeData[nodeIndex].count;

		for(unsigned int i=startIndex;i<endIndex;i++)
		{
			//fix tabs
			for(unsigned int j=0;j<level;out<<"\t",j++);
			//print the data
			out<<"\t"<<data[indices[i]]<<std::endl;
		}
	}
	else
	{
		for(unsigned int j=0;j<level;out<<"\t",j++);
		////print dividing axis
		//switch(treeData[nodeIndex].dividingAxis)
		//{
		//	case 0:
		//		out<<level<<" x ";
		//		break;
		//	case 1:
		//		out<<level<<" y ";
		//		break;
		//	case 2:
		//		out<<level<<" z ";
		//		break;
		//	default:
		//		out<<"Error";
		//		break;
		//}
		//out<<treeData[nodeIndex].count<<"\n";

		//go throught all the children
		level++;

		//compute the starting index of the children
		unsigned int leftChildNodeIndex=treeData[nodeIndex].index;
		unsigned int rightChildNodeIndex=leftChildNodeIndex+1;

		printHelper(out,treeData,leftChildNodeIndex,data,indices,level);
		printHelper(out,treeData,rightChildNodeIndex,data,indices,level);
		
	}			
	return out;
}
//split the data
void kdTree3D::splitCPU(kdTreeData3D::levelNodeData *&nodeData, unsigned int &nodesCount, const unsigned int &levelStartIndex, const unsigned int &levelCount,const unsigned int &childStartIndex,unsigned int &childLevelCount,unsigned int *&childIndices,unsigned int &childIndexCount,unsigned int &freeChildIndicesIndex,unsigned int &occupiedChildIndicesIndex,const floatFace *data)
{
	kdTreeData3D::levelNodeData *parentData=&nodeData[levelStartIndex];
	kdTreeData3D::levelNodeData *childData=&nodeData[childStartIndex];

	////////////////////////////////////////////////////////////
	//find total element that need to be added
	unsigned int totalElements=0;
	for(unsigned int i=0;i<levelCount;i++)
	{
		//get the current data
		levelNodeData &currNode=parentData[i];

		//add the count
		totalElements+=currNode.count;
	}

	////////////////////////////////////////////////////////////
	//allocate space to find where each data goes to
	char *nodeAssignment=new char[totalElements];
	memset(nodeAssignment,0,totalElements);

	unsigned int *nodeAssignmentIndex=new unsigned int[levelCount];
	memset(nodeAssignmentIndex,0,levelCount*sizeof(unsigned int));

	unsigned int *childNodeAssignmentCount=new unsigned int[levelCount*2];
	memset(childNodeAssignmentCount,0,levelCount*2*sizeof(unsigned int));

	////////////////////////////////////////////////////////////
	//assign each element to node
	unsigned int index=0;
	//find which side each nodes goes to
	for(unsigned int i=0;i<levelCount;i++)
	{
		//get the current data
		levelNodeData &currNode=parentData[i];

		//create child boundary
		floatBox lChildBoundary=currNode.getLeftChildBoundary();
		floatBox rChildBoundary=currNode.getRightChildBoundary();
		
		//save the index
		nodeAssignmentIndex[i]=index;

		//process each element
		for(unsigned int j=0;j<currNode.count;j++)
		{
			const floatFace &currData=data[indices[currNode.childIndex+j]];

			if(lChildBoundary.doesIntersect(currData))
			{
				nodeAssignment[index]=nodeAssignment[index]|1;
				childNodeAssignmentCount[i*2]++;
			}
			if(rChildBoundary.doesIntersect(currData))
			{
				nodeAssignment[index]=nodeAssignment[index]|2;
				childNodeAssignmentCount[i*2+1]++;
			}
			index++;			
		}
	}


	////////////////////////////////////////////////////////////
	//compute total new nodes that will be added
	unsigned int totalChildNodeAdded=0;
	unsigned int totalDataInChildNodes=0;
	//triangle are sub divided into segments
	for(unsigned int i=0;i<levelCount;i++)
	{
		//get the current data
		levelNodeData &currNode=parentData[i];

		unsigned int temp=(childNodeAssignmentCount[i*2]+childNodeAssignmentCount[i*2+1]);
		
		//calculate
		totalDataInChildNodes+=temp;

		//do not sub-divide the node
		if(childNodeAssignmentCount[i*2]==0 || 
			childNodeAssignmentCount[i*2+1]==0 || 
			(temp<optimunDataCount)||
			(temp>=2*currNode.count)
			)
			continue;

		totalChildNodeAdded+=2;
	}

	//total nodes
	unsigned int totalNodes=totalChildNodeAdded+childStartIndex;
	//increase the size if needed
	if(totalNodes>=nodesCount)
	{
		kdTreeData3D::levelNodeData *tempNodeData=new kdTreeData3D::levelNodeData[totalNodes];
		memcpy(tempNodeData,nodeData,sizeof(kdTreeData3D::levelNodeData)*nodesCount);
		delete[] nodeData;
		nodeData=tempNodeData;
		nodesCount=totalNodes;
		
		//update the pointers
		parentData=&nodeData[levelStartIndex];
		childData=&nodeData[childStartIndex];
	}

	//increase the size of needed
	if(totalDataInChildNodes>childIndexCount-freeChildIndicesIndex)
	{
		childIndexCount=freeChildIndicesIndex+totalDataInChildNodes;
		unsigned int *tempChildIndices=new unsigned int[childIndexCount];
		memcpy(tempChildIndices,childIndices,sizeof(unsigned int)*occupiedChildIndicesIndex);
		delete[] childIndices;
		childIndices=tempChildIndices;
	}

	//allocate space to temporarly hold the curr indices
	unsigned int *tempChildIndicesCurrLevel=new unsigned int[(occupiedChildIndicesIndex-freeChildIndicesIndex)];
	memcpy(tempChildIndicesCurrLevel,&childIndices[freeChildIndicesIndex],sizeof(unsigned int)*(occupiedChildIndicesIndex-freeChildIndicesIndex));


	///////////////////////////////////////////////////////////////
	//copy child nodes that doesn't need further processing
	unsigned int tempFreeChildIndicesIndex=freeChildIndicesIndex;
	for(unsigned int i=0;i<levelCount;i++)
	{
		//get the current data
		levelNodeData &currNode=parentData[i];

		//do not sub-divide the node
		if(childNodeAssignmentCount[i*2]==0 || 
			childNodeAssignmentCount[i*2+1]==0 || 
			((childNodeAssignmentCount[i*2]+childNodeAssignmentCount[i*2+1])<optimunDataCount)||
			((childNodeAssignmentCount[i*2]+childNodeAssignmentCount[i*2+1])>=2*currNode.count)
			)
		{
			//copy the data
			memcpy(&childIndices[freeChildIndicesIndex],&tempChildIndicesCurrLevel[currNode.childIndex-tempFreeChildIndicesIndex],sizeof(unsigned int)*currNode.count);
			//save the childrens index
			currNode.childIndex=freeChildIndicesIndex;
			//increase the current index
			freeChildIndicesIndex+=currNode.count;			
		}
	}


	///////////////////////////////////////////////////////////////
	//create and copy child nodes that doesn't need further processing
	unsigned int currIndexPos=0;
	for(unsigned int i=0;i<levelCount;i++)
	{
		//get the current data
		levelNodeData &currNode=parentData[i];

		//do not sub-divide the node
		if(childNodeAssignmentCount[i*2]==0 || 
			childNodeAssignmentCount[i*2+1]==0 || 
			((childNodeAssignmentCount[i*2]+childNodeAssignmentCount[i*2+1])<optimunDataCount)||
			((childNodeAssignmentCount[i*2]+childNodeAssignmentCount[i*2+1])>=2*currNode.count)
			)
			continue;

							
		unsigned int leftIndex=0,rightIndex=0;
		//process left each element
		//create child boundary
		childData[childLevelCount].boundary=currNode.getLeftChildBoundary();
		childData[childLevelCount].count=childNodeAssignmentCount[i*2];
		childData[childLevelCount].isLeaf=true;
		index=nodeAssignmentIndex[i];
		childData[childLevelCount].childIndex=freeChildIndicesIndex+currIndexPos;
		for(unsigned int j=0;j<currNode.count;j++)
		{
			//if the data is in left child
			if(nodeAssignment[index]&1)
				childIndices[freeChildIndicesIndex+currIndexPos++]=tempChildIndicesCurrLevel[currNode.childIndex-tempFreeChildIndicesIndex+j];
			index++;
		}

		//process right each element
		//right child
		childData[childLevelCount+1].boundary=currNode.getRightChildBoundary();
		childData[childLevelCount+1].count=childNodeAssignmentCount[i*2+1];
		childData[childLevelCount+1].isLeaf=true;
		index=nodeAssignmentIndex[i];
		childData[childLevelCount+1].childIndex=freeChildIndicesIndex+currIndexPos;
		for(unsigned int j=0;j<currNode.count;j++)
		{
			
			//if the data is in right child
			if(nodeAssignment[index]&2)
				childIndices[freeChildIndicesIndex+currIndexPos++]=tempChildIndicesCurrLevel[currNode.childIndex-tempFreeChildIndicesIndex+j];
			index++;
		}

		//clear the parent
		parentData[i].isLeaf=false;
		parentData[i].count=0;
		parentData[i].index=childStartIndex+childLevelCount;	//update the index
		parentData[i].childIndex=0;

		childLevelCount+=2;
	}

	//update the value
	occupiedChildIndicesIndex=freeChildIndicesIndex+currIndexPos;

	delete[] tempChildIndicesCurrLevel;
	delete[] nodeAssignmentIndex;
	delete[] childNodeAssignmentCount;
	delete[] nodeAssignment;

}
//searches for the nodes in cpu
int *kdTree3D::findHelperCPU(floatFace *keys, unsigned int keysCount)
{
	if(count==0)
		return NULL;
	
	//allocate data for results
	int *keyIndices=new int[keysCount];

	//search for each keys
	for(unsigned int pos=0;pos<keysCount;pos++)
	{
		//copy the default value
		keyIndices[pos]=-1;
		floatFace &currKey=keys[pos];

		unsigned int nodeIndex=0;
		while(true)
		{
			//check if the node is a leaf
			if(treeData[nodeIndex].isLeaf)
			{
				unsigned int startIndex=(unsigned)treeData[nodeIndex].index;
				unsigned int endIndex=startIndex+treeData[nodeIndex].count;

				for(unsigned int i=startIndex;i<endIndex;i++)
				{
					if(data[indices[i]]==currKey)
					{
						keyIndices[pos]=(signed)i;	//copy the index
						break;
					}
				}
				break;
			}
			else
			{
				//compute the starting index of the children
				unsigned int leftChildNodeIndex=treeData[nodeIndex].index;
				unsigned int rightChildNodeIndex=leftChildNodeIndex+1;

				if(treeData[leftChildNodeIndex].boundary.doesIntersect(currKey))
					nodeIndex=leftChildNodeIndex;
				else if(treeData[rightChildNodeIndex].boundary.doesIntersect(currKey))
					nodeIndex=rightChildNodeIndex;
				else
					break;
				
			}
		}
	}

	return keyIndices;
}
//searches for the nodes in cpu
int *kdTree3D::findNearestKHelperCPU(float3 *keys, unsigned int keysCount, unsigned int k)
{
	if(count==0)
		return NULL;
	
	//number of result
	unsigned int resultsCount=keysCount*k;

	//allocate data for results
	int *keyIndices=new int[resultsCount];

	//array to save the right tree indices
	int *farChildrenIndices=new int[treeHeight];

	//temp variables
	int *tempIndices=new int[k];
	float *distance=new float[k];

	//search for each keys
	for(unsigned int pos=0;pos<keysCount;pos++)
	{
		//prepare the variables
		for(unsigned int j=0;j<k;j++)
		{
			tempIndices[j]=-1;
			distance[j]=100000000000.0f;	//put some really big number
		}

		float3 &currKey=keys[pos];
		unsigned int nodeIndex=0;
		int level=0;
		bool firstLeafReached=false;

		while(level>=0)
		{
			bool goLevelUp=false;
			//check if the node is a leaf
			if(treeData[nodeIndex].isLeaf)
			{
				firstLeafReached=true;

				unsigned int startIndex=(unsigned)treeData[nodeIndex].index;
				unsigned int endIndex=startIndex+treeData[nodeIndex].count;

				for(unsigned int i=startIndex;i<endIndex;i++)
				{
					float currDistance=data[indices[i]].distance(currKey);

					//is the index is bigger than current radius
					if(distance[k-1]<currDistance)
						continue;

					int insertIndex=-1;
					//check if the distance is smaller
					for(int j=(signed)k-1;j>=0;j--)
					{
						if(currDistance>distance[j])
							break;
						insertIndex=j;
					}

					//now insert the element
					if(insertIndex==-1)
						continue;

					if(tempIndices[insertIndex]!=-1)
					{
						//first create a space
						for(int j=(signed)k-1;j>insertIndex;j--)
						{	
							tempIndices[j]=tempIndices[j-1];
							distance[j]=distance[j-1];

						}
					}

					//save index and the distance
					tempIndices[insertIndex]=i;
					distance[insertIndex]=currDistance;
				}

				//go level up
				goLevelUp=true;
			}
			else if(firstLeafReached)	//if the first leaf has not been reached we need to find it
			{
				//compute the starting index of the children
				unsigned int leftChildNodeIndex=treeData[nodeIndex].index;
				unsigned int rightChildNodeIndex=leftChildNodeIndex+1;

				float lDistance=treeData[leftChildNodeIndex].boundary.distance(currKey);
				float rDistance=treeData[rightChildNodeIndex].boundary.distance(currKey);

				if(lDistance<distance[k-1])
				{
					//go left and put the car children to the right
					nodeIndex=leftChildNodeIndex;

					//check if right is within reach too
					if(rDistance<distance[k-1])
					{						
						farChildrenIndices[level]=rightChildNodeIndex;
						
						//increase the level
						level++;
					}
				}
				else if(rDistance<distance[k-1])
				{
					//go left and put the car children to the right
					nodeIndex=rightChildNodeIndex;
				}
				else
				{
					//go level up
					goLevelUp=true;
				}

			}
			else
			{
				//compute the starting index of the children
				unsigned int leftChildNodeIndex=treeData[nodeIndex].index;
				unsigned int rightChildNodeIndex=leftChildNodeIndex+1;

				if(treeData[leftChildNodeIndex].boundary.doesIntersect(currKey))
				{
					nodeIndex=leftChildNodeIndex;
					farChildrenIndices[level]=rightChildNodeIndex;
					level++;//increase the level
				}
				else if(treeData[rightChildNodeIndex].boundary.doesIntersect(currKey))
				{
					farChildrenIndices[level]=leftChildNodeIndex;
					nodeIndex=rightChildNodeIndex;
					level++;//increase the level
				}
				else
				{
					//go level up
					goLevelUp=true;
				}
			}

			if(goLevelUp)
			{
				//decrease level and
				//check if the node intersects the search radius
				do
				{
					//decrease the level
					level--;

					//get out of the whole loop
					if(level<0)
						break;

					//else retrive the other child
					nodeIndex=farChildrenIndices[level];

					//check if the distance to the box is bigger than expected
					if(treeData[nodeIndex].boundary.distance(currKey)<distance[k-1])
						break;
				}while(true);
			}
		}

		//copy the indices
		for(unsigned int j=0;j<k;j++)
			keyIndices[pos*k+j]=tempIndices[j];

	}

	delete[] tempIndices;
	delete[] distance;
	delete[] farChildrenIndices;

	return keyIndices;
}
//searches for nodes within a ridius
void kdTree3D::findRadiusHelperCPU(float3 *keys, unsigned int keysCount, float radius, int *&keyIndices, unsigned int *&prefixSumResultsCount,unsigned int &totalResultsCount)
{
	if(count==0)
		return;

	//make indices null
	keyIndices=NULL;	

	//number of result
	unsigned int *resultsCount=new unsigned int[keysCount];
	prefixSumResultsCount=new unsigned int[keysCount];
	memset(resultsCount,0,sizeof(unsigned int)*keysCount);

	//array to save the right tree indices
	int *farChildrenIndices=new int[treeHeight];
	
	//////////////////////////////////////////////////////////////////////////////
	//find the total number of keys within the search region

	//search for each keys
	for(unsigned int pos=0;pos<keysCount;pos++)
	{
		float3 &currKey=keys[pos];
		unsigned int nodeIndex=0;
		int level=0;
		bool firstLeafReached=false;

		while(level>=0)
		{
			bool goLevelUp=false;
			//check if the node is a leaf
			if(treeData[nodeIndex].isLeaf)
			{
				firstLeafReached=true;

				unsigned int startIndex=(unsigned)treeData[nodeIndex].index;
				unsigned int endIndex=startIndex+treeData[nodeIndex].count;

				for(unsigned int i=startIndex;i<endIndex;i++)
				{
					float currDistance=data[indices[i]].distance(currKey);

					//is the index is bigger than current radius
					if(radius<currDistance)
						continue;
					
					//increase the result count
					resultsCount[pos]++;
				}

				//go level up
				goLevelUp=true;
			}
			else if(firstLeafReached)	//if the first leaf has not been reached we need to find it
			{
				//compute the starting index of the children
				unsigned int leftChildNodeIndex=treeData[nodeIndex].index;
				unsigned int rightChildNodeIndex=leftChildNodeIndex+1;

				float lDistance=treeData[leftChildNodeIndex].boundary.distance(currKey);
				float rDistance=treeData[rightChildNodeIndex].boundary.distance(currKey);

				if(lDistance<radius)
				{
					//go left and put the car children to the right
					nodeIndex=leftChildNodeIndex;

					//check if right is within reach too
					if(rDistance<radius)
					{						
						farChildrenIndices[level]=rightChildNodeIndex;
						
						//increase the level
						level++;
					}
				}
				else if(rDistance<radius)
				{
					//go left and put the car children to the right
					nodeIndex=rightChildNodeIndex;
				}
				else
				{
					//go level up
					goLevelUp=true;
				}

			}
			else
			{
				//compute the starting index of the children
				unsigned int leftChildNodeIndex=treeData[nodeIndex].index;
				unsigned int rightChildNodeIndex=leftChildNodeIndex+1;

				if(treeData[leftChildNodeIndex].boundary.doesIntersect(currKey))
				{
					nodeIndex=leftChildNodeIndex;
					farChildrenIndices[level]=rightChildNodeIndex;
					level++;//increase the level
				}
				else if(treeData[rightChildNodeIndex].boundary.doesIntersect(currKey))
				{
					farChildrenIndices[level]=leftChildNodeIndex;
					nodeIndex=rightChildNodeIndex;
					level++;//increase the level
				}
				else
				{
					//go level up
					goLevelUp=true;
				}
			}

			if(goLevelUp)
			{
				//decrease level and
				//check if the node intersects the search radius
				do
				{
					//decrease the level
					level--;

					//get out of the whole loop
					if(level<0)
						break;

					//else retrive the other child
					nodeIndex=farChildrenIndices[level];

					//check if the distance to the box is bigger than expected
					if(treeData[nodeIndex].boundary.distance(currKey)<radius)
						break;
				}while(true);
			}
		}
	}
	//////////////////////////////////////////////////////////////////////////////
	//find total search result

	//prefix sum
	prefixSumResultsCount[0]=0;
	for(unsigned int i=1;i<keysCount;i++)
		prefixSumResultsCount[i]=resultsCount[i-1]+prefixSumResultsCount[i-1];
	//compute total results count
	totalResultsCount=prefixSumResultsCount[keysCount-1]+resultsCount[keysCount-1];


	//////////////////////////////////////////////////////////////////////////////
	//perform search

	//allocate data for results
	keyIndices=new int[totalResultsCount];
	for(unsigned int i=0;i<totalResultsCount;i++)
		keyIndices[i]=-1;

	//search for each keys
	for(unsigned int pos=0;pos<keysCount;pos++)
	{
		float3 &currKey=keys[pos];
		unsigned int nodeIndex=0;
		unsigned int insertIndex=prefixSumResultsCount[pos];
		int level=0;
		bool firstLeafReached=false;

		while(level>=0)
		{
			bool goLevelUp=false;
			//check if the node is a leaf
			if(treeData[nodeIndex].isLeaf)
			{
				firstLeafReached=true;

				unsigned int startIndex=(unsigned)treeData[nodeIndex].index;
				unsigned int endIndex=startIndex+treeData[nodeIndex].count;

				for(unsigned int i=startIndex;i<endIndex;i++)
				{
					float currDistance=data[indices[i]].distance(currKey);

					//is the index is bigger than current radius
					if(radius<currDistance)
						continue;

					//save index and the distance
					keyIndices[insertIndex]=i;

					//increase the insert index
					insertIndex++;
				}
				//go level up
				goLevelUp=true;
			}
			else if(firstLeafReached)	//if the first leaf has not been reached we need to find it
			{
				//compute the starting index of the children
				unsigned int leftChildNodeIndex=treeData[nodeIndex].index;
				unsigned int rightChildNodeIndex=leftChildNodeIndex+1;

				float lDistance=treeData[leftChildNodeIndex].boundary.distance(currKey);
				float rDistance=treeData[rightChildNodeIndex].boundary.distance(currKey);

				if(lDistance<radius)
				{
					//go left and put the car children to the right
					nodeIndex=leftChildNodeIndex;

					//check if right is within reach too
					if(rDistance<radius)
					{						
						farChildrenIndices[level]=rightChildNodeIndex;
						
						//increase the level
						level++;
					}
				}
				else if(rDistance<radius)
				{
					//go left and put the car children to the right
					nodeIndex=rightChildNodeIndex;
				}
				else
				{
					//go level up
					goLevelUp=true;
				}

			}
			else
			{
				//compute the starting index of the children
				unsigned int leftChildNodeIndex=treeData[nodeIndex].index;
				unsigned int rightChildNodeIndex=leftChildNodeIndex+1;

				if(treeData[leftChildNodeIndex].boundary.doesIntersect(currKey))
				{
					nodeIndex=leftChildNodeIndex;
					farChildrenIndices[level]=rightChildNodeIndex;
					level++;//increase the level
				}
				else if(treeData[rightChildNodeIndex].boundary.doesIntersect(currKey))
				{
					farChildrenIndices[level]=leftChildNodeIndex;
					nodeIndex=rightChildNodeIndex;
					level++;//increase the level
				}
				else
				{
					//go level up
					goLevelUp=true;
				}
			}

			if(goLevelUp)
			{
				//decrease level and
				//check if the node intersects the search radius
				do
				{
					//decrease the level
					level--;

					//get out of the whole loop
					if(level<0)
						break;

					//else retrive the other child
					nodeIndex=farChildrenIndices[level];

					//check if the distance to the box is bigger than expected
					if(treeData[nodeIndex].boundary.distance(currKey)<radius)
						break;
				}while(true);
			}
		}
	}



	//////////////////////////////////////////////////////////////////////////////
	delete[] resultsCount;
	delete[] farChildrenIndices;
}

//intersect ray cpu helper
void kdTree3D::intersectRayHelperCPU(floatRayData &currRay,unsigned int k,int *resultIndices, float *t,int *farChildrenIndices,int *tempIndices,float *distance)
{
	//prepare the variables
	for(unsigned int j=0;j<k;j++)
	{
		tempIndices[j]=-1;
		distance[j]=100000000000.0f;	//put some really big number
	}

	//change this
	unsigned int nodeIndex=0;
	int level=0;
	bool firstLeafReached=false;

	while(level>=0)
	{
		bool goLevelUp=false;
		//check if the node is a leaf
		if(treeData[nodeIndex].isLeaf)
		{
			firstLeafReached=true;
			unsigned int startIndex=(unsigned)treeData[nodeIndex].index;
			unsigned int endIndex=startIndex+treeData[nodeIndex].count;

			for(unsigned int i=startIndex;i<endIndex;i++)
			{
				float currDistance=data[indices[i]].getIntersectDistance(currRay);

				//skip if the distance is less than zero
				if(currDistance<0)
					continue;

				//is the index is bigger than current radius
				if(distance[k-1]<currDistance)
					continue;

				int insertIndex=-1;
				//check if the distance is smaller
				for(int j=(signed)k-1;j>=0;j--)
				{
					if(currDistance>distance[j])
						break;
					insertIndex=j;
				}

				//now insert the element
				if(insertIndex==-1)
					continue;

				if(tempIndices[insertIndex]!=-1)
				{
					//first create a space
					for(int j=(signed)k-1;j>insertIndex;j--)
					{	
						tempIndices[j]=tempIndices[j-1];
						distance[j]=distance[j-1];

					}
				}

				//save index and the distance
				tempIndices[insertIndex]=indices[i];
				distance[insertIndex]=currDistance;
			}

			//go level up
			goLevelUp=true;
		}
		else if(firstLeafReached)	//if the first leaf has not been reached we need to find it
		{
			//compute the starting index of the children
			unsigned int leftChildNodeIndex=treeData[nodeIndex].index;
			unsigned int rightChildNodeIndex=leftChildNodeIndex+1;

			float lDistance=treeData[leftChildNodeIndex].boundary.getIntersectDistance(currRay);
			float rDistance=treeData[rightChildNodeIndex].boundary.getIntersectDistance(currRay);

			if(lDistance>=0 && lDistance<distance[k-1])
			{
				if(rDistance>=0 && rDistance<distance[k-1])
				{
					if(lDistance<rDistance)
					{
						nodeIndex=leftChildNodeIndex;
						farChildrenIndices[level]=rightChildNodeIndex;
					}
					else
					{
						farChildrenIndices[level]=leftChildNodeIndex;
						nodeIndex=rightChildNodeIndex;
					}
					//increase the level
					level++;
				}
				else
				{
					nodeIndex=leftChildNodeIndex;
				}
			}
			else if(rDistance>=0 && rDistance<distance[k-1])
			{
				nodeIndex=rightChildNodeIndex;
			}
			else
			{
				//go level up
				goLevelUp=true;
			}


		}
		else
		{
			//compute the starting index of the children
			unsigned int leftChildNodeIndex=treeData[nodeIndex].index;
			unsigned int rightChildNodeIndex=leftChildNodeIndex+1;

			float lDistance=treeData[leftChildNodeIndex].boundary.getIntersectDistance(currRay);
			float rDistance=treeData[rightChildNodeIndex].boundary.getIntersectDistance(currRay);

			if(lDistance>=0)
			{
				if(rDistance>=0)
				{
					if(lDistance<rDistance)
					{
						nodeIndex=leftChildNodeIndex;
						farChildrenIndices[level]=rightChildNodeIndex;
					}
					else
					{
						farChildrenIndices[level]=leftChildNodeIndex;
						nodeIndex=rightChildNodeIndex;
					}
					//increase the level
					level++;
				}
				else
				{
					nodeIndex=leftChildNodeIndex;
				}
			}
			else if(rDistance>=0)
			{
				nodeIndex=rightChildNodeIndex;
			}
			else
			{
				//go level up
				goLevelUp=true;
			}
		}

		if(goLevelUp)
		{
			//decrease level and
			//check if the node intersects the search radius
			do
			{
				//decrease the level
				level--;

				//get out of the whole loop
				if(level<0)
					break;

				//else retrive the other child
				nodeIndex=farChildrenIndices[level];

				float dist=treeData[nodeIndex].boundary.getIntersectDistance(currRay);

				//check if the distance to the box is bigger than expected
				if(dist>-1 && dist<distance[k-1])
					break;
			}while(true);
		}
	}

	//copy results
	memcpy(resultIndices,tempIndices,sizeof(int)*k);
	memcpy(t,distance,sizeof(float)*k);
}